/*
 * flock.cpp
 *
 *  Created on: 15.02.2012
 *      Author: ignacio
 */



#include "flock.h"
#include <exception>
using std::exception;
#include <iostream>
using std::cout;
using std::endl;
#include <sstream>

namespace neurignacio{

Vector3 TargetLeptonPointerContainer::averagePosition() const
{
	Vector3 averagePosition=NULL_VECTOR;
	for (LeptonPointerContainer::const_iterator lepton=targetContainer.begin();lepton!=targetContainer.end(); ++lepton)
		averagePosition += (*lepton)->position;
//	cout << "Average Position: " << averagePosition << " targetContainer.size(): " << targetContainer.size() << endl;
	return averagePosition / targetContainer.size();
}

Vector3 TargetLeptonPointerContainer::averageVelocity() const
{
	Vector3 averageVelocity=NULL_VECTOR;
	for (LeptonPointerContainer::const_iterator lepton=targetContainer.begin();lepton!=targetContainer.end(); ++lepton)
		averageVelocity += (*lepton)->velocity;
	return averageVelocity / targetContainer.size();
}

void Flock::getSteering(SteeringOutput& output)
// 1. find the boids that are within the field of view and the horizont of the character [=Neighbors]
// 2. calculate the center of Mass and average Velocity
// 3. make virtual target lepton
// 4. Flock dependent behaviours are set to avoidCollision, seek and look.
//	  Each is weighted 1/3.
//    AvoidCollision will affect all the neighbours and result will be 1/3 of the overall behaviour
// 5. get the overall steering
{
	std::stringstream out;
	output.clear(); // erase output!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!1
//	output.resize(characterContainer.size());
	out << "\t$$$$$$$$$$ FLOCK.getSteering() - Line " << __LINE__ << endl;
	angle halfFieldOfView = fieldOfView / 2;
	out << " half FOV: " << halfFieldOfView << endl;

	// FOR EACH OF THE BOIDS IN THE FLOCK
	unsigned int boidCounter=0;	// For using as output index at the end
	for (GluonPointerContainer::iterator boidCharacter=characterContainer.begin(); boidCharacter!=characterContainer.end(); ++boidCharacter, ++boidCounter)
	{
		out << "\tMe: " << (*boidCharacter)->position << " Horizont: " << horizont << " FOV: " << fieldOfView;
		// 1. Find my neighbours
		out << "\t1. Find neighbours" << endl;
		LeptonPointerContainer neighbourContainer;
		uint16 numOfNeighbours=0; // Count how many neighbours I have (for debuging purposes)
		int neighbourCounter=0; // for debuging
		real distanceToClosestNeighbour = REAL_MAX;	// I want to find which neighbour is the closest to avoid collision?
		GluonPointerContainer::iterator closestNeighbour = characterContainer.end();	// pointer to the closest neighbour. Initialized to the end of container (=NULL)
		// FROM MY FLOCK, WHO IS MY NEIGHBOUR?
		for (GluonPointerContainer::iterator neighbour=characterContainer.begin();neighbour!=characterContainer.end(); ++neighbour, ++neighbourCounter)
		{
			out << "\tBoid #" << neighbourCounter << " position: "<< (*neighbour)->position;
			if (boidCharacter!=(neighbour) ) // I'm not myself!
			{
				Vector3 distance = (*neighbour)->position;	// Distance between me and my neighbour
				distance -= (*boidCharacter)->position;
				real distanceLength=distance.length();
				out << " distance: " << distanceLength;
				out << " angle: " << neurignacio::abs( (*boidCharacter)->orientation-distance.angle() );
				// My neighbour is close enough to me
				// and within my field of view
				if (distanceLength<=horizont && neurignacio::abs( (*boidCharacter)->orientation-distance.angle() ) <= halfFieldOfView)
				{
					neighbourContainer.push_back(*neighbour);
					if (distanceLength<distanceToClosestNeighbour)
					{
						closestNeighbour=neighbour;
						distanceToClosestNeighbour=distanceLength;
					}
					out << " Neighbour!";
					++numOfNeighbours;
			} // end if (distance<=horizont)

			} // end if (boid!=neighbour)
			out << endl;
		} // end for neighbour
		if (!neighbourContainer.size()) // No neighbour was found (I'm alone and I have no flock to follow) !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! CHECK !!!!!!!!!!!!!!!!!
			return;
		out << "Size of NeuighbourContainer: " << neighbourContainer.size() << endl;
		TargetLeptonPointerContainer neighbourhood(&neighbourContainer);
		out << "Size of Neuighbourhood: " << neighbourhood.targetContainer.size() << endl;

		// 2. Calculate Average
		Vector3 centerOfMass = neighbourhood.averagePosition();
		Vector3 averageVelocity = neighbourhood.averageVelocity();
		out << "\t\tCenter of Mass: " << centerOfMass << " Average Velocity: " << averageVelocity << endl;

		// 3. Make Virtual Target
		Lepton virtualTarget;
		virtualTarget.position = centerOfMass;
		virtualTarget.velocity = averageVelocity;

		// 4. Apply new target to all behaviours in flock and corresponding Steering
		SteeringOutput steering;
		steering.clear();
		neurignacio::MovementBehaviourPointerContainer virtualMovementBehaviour; // Reset the movement Behaviour

		//	 a) AvoidCollision
		neurignacio::CollisionAvoidance* virtualCollision = new neurignacio::CollisionAvoidance[neighbourContainer.size()];
//		neurignacio::Separation* virtualSeparation = new neurignacio::Separation[neighbourContainer.size()];

		for (unsigned int counter=0; counter<neighbourContainer.size(); ++counter)
		{
			virtualCollision[counter].setCollisionRadius(this->collision.collisionRadius);
			virtualCollision[counter].setMaxPrediction(this->collision.getMaxPrediction());// Copy collision behaviour defined in flock to the virtual collision behaviour
			virtualCollision[counter].setTarget(neighbourContainer[counter]);	// Assigns the target to this behaviour
			virtualCollision[counter].weight = 1.0f; //neighbourContainer.size();	// Total combined weight of collision = 1.0f
			virtualMovementBehaviour.push_back(&virtualCollision[counter]);


//	        virtualSeparation[counter].thresholdDistance = this->separation.thresholdDistance;
//	        virtualSeparation[counter].decayCoefficient = this->separation.decayCoefficient;
		}


		//	b) Arrive
		neurignacio::Arrive virtualArrive(this->arrive);
//		virtualArrive.setTarget(&virtualTarget); // if commented this line, target for arrive will be parent otherwise virtualTarget
		virtualMovementBehaviour.push_back(&virtualArrive);

		//	c) Look
		neurignacio::Look virtualLook(look);
		virtualMovementBehaviour.push_back(&look);

		//  5. Make Virtual Character
		Gluon virtualCharacter(**boidCharacter);
		virtualCharacter.movementBehaviour.clear();
		//	6. Calculate Combined Steering for Flock
		virtualCharacter.addMovementBehaviour(virtualMovementBehaviour);
		out << "VIRTUAL CHARACTER: "<<virtualCharacter << endl;
		virtualCharacter.getSteering();
		(*boidCharacter)->initialSteering = virtualCharacter.steering;
	//	output[boidCounter]=virtualCharacter.steering;

		// 7. Get Overall Steering
		// need to average? => can be included in weight afterwards
		// What about weight and priority (maybe can be calculated by copying character into a virtual character?)
		// output /= this->movementBehaviour.size();
		out << "\t\tFinal Output Steering: ";
		out << virtualCharacter.steering << endl;

		// 8. clean up virtuals
		delete [] virtualCollision;
//		delete [] virtualSeparation;

//		cout << out.str();
	} // end for boid
} // end Flock::getSteering


} // end namespace neurignacio
